#include <buola/image/io.h>
#include <thread>
#include <buola/image/algorithm/detail/opencv.h>
#include <buola/image/cimagegc.h>
#include <buola/app/ccmdline.h>
#include <buola/cv/features/cdetector.h>
#include <buola/cv/features/cmatcher.h>
#include <buola/cv/features/cmatchview.h>
#include <buola/scene/cscene.h>
#include <buola/scene/geodes/cpointcloud.h>
#include <buola/scene/geodes/ccube.h>
#include <buola/scene/cperspectivecamera.h>
#include <buola/scene/csceneview.h>
#include <buola/scene/controllers/ccameracontroller.h>
#include <buola/scene/transform/cmattransform.h>
#include <buola/gui/ctoolkit.h>
#include <buola/cv/geometry/transform.h>

using namespace buola;

int main(int pNArg,char **pArgs)
{
    buola_init(pNArg,pArgs);

    if(cmd_line().ArgCount()<2)
    {
        msg_info() << "please specify filename and count\n";
        return 1;
    }

    try
    {
        int lNum=to_<int>(cmd_line().Arg(1));
        
        std::vector<img::CImage_rgb8> lSrc(lNum);
        std::vector<cvi::CImage_point_rgb8> lCloud(lNum);
        std::vector<img::CImage_gray8> lImg(lNum),lMask(lNum);
        std::vector<cvi::CFeatureSet> lSet(lNum);
        cvi::CSURFDetector lSURF;
        
        for(int i=0;i<lNum;i++)
        {
            img::load(lSrc[i],io::file(cmd_line().Arg(0)+to_string(i)+".pcd"));
            img::load(lCloud[i],io::file(cmd_line().Arg(0)+to_string(i)+".pcd"));
            lImg[i].Recreate(lSrc[i].Size());
            img::convert_pixels(lSrc[i],lImg[i]);

            lMask[i].Recreate(lSrc[i].Size());
            for(int x=0;x<640;x++)
            {
                for(int y=0;y<480;y++)
                {
                    lMask[i](x,y)=is_nan(lCloud[i](x,y)->mPoint.x())?0:255;
                }
            }
            
            start_timer();
            lSet[i]=lSURF.Detect(lImg[i],lMask[i]);
            end_timer();

            msg_info() << lSet[i].size() << " " << lSet[i].Features().Rows() << "x" << lSet[i].Features().Cols() << "\n";
        }
        
//         cvi::CMatchView lView;
//         lView.SetImages(img::share(lSrc1),img::share(lSrc2));
//         lView.SetFeatures(cvi::share(lSet1),cvi::share(lSet2));
//         lView.SetMatches(lGoodMatches);
//         lView.Map();
        
        gui::get_toolkit()->GetOpenGLGC()->MakeCurrent();

        //create the scene
        scene::PScene lScene=new scene::CScene;

        mat::CMat34d lTrans=mat::identity_3d();
        
        for(int i=0;i<lNum-1;i++)
        {
            cvi::CFeatureMatches lMatches=bf_match(lSet[i],lSet[i+1]);
            lMatches.Sort();
            msg_info() << lMatches.size() << " matches\n";

            std::vector<mat::CVec3d> lLeft,lRight;
        
            for(int j=0;j<lMatches.size();j++)
            {
                const cvi::CKeyPoint &lKP1=lMatches.KeyPoint1(j);
                const cvi::CKeyPoint &lKP2=lMatches.KeyPoint2(j);
                mat::CVec3f lP1=lCloud[i](lKP1.Point_i())->mPoint;
                mat::CVec3f lP2=lCloud[i+1](lKP2.Point_i())->mPoint;
                if(is_nan(lP1.z())||is_nan(lP2.z())) continue;
                float lCoeff=lP1.z()*lKP1.Size()/lP2.z()/lKP2.Size();
                if(lCoeff>1.1f||lCoeff<0.9f) continue;
                lLeft.push_back(mat::CVec3d(lP1));
                lRight.push_back(mat::CVec3d(lP2));
            }
        
            msg_info() << lLeft.size() << "," << lRight.size() << " RANSAC points\n";

            start_timer();
            cvi::estimate_affine_transform(lLeft,lRight,0.1,0.998);
            end_timer();
            start_timer();
            mat::CMat34d lNewTrans=cvi::estimate_rigid_transform(lLeft,lRight,0.1,0.998);
            msg_info() << "new trans: " << lNewTrans << "\n";
            end_timer();
            lTrans=lTrans*inverse(lNewTrans);

            if(i==0)
            {
                scene::PPointCloud lSceneCloud=new scene::CPointCloud;
                lSceneCloud->SetCloud(cvi::CCloud_rgb8(lCloud[i]));
                lScene->World()->AddObject(lSceneCloud);
            }
            
            scene::PPointCloud lSceneCloud=new scene::CPointCloud;
            lSceneCloud->SetCloud(cvi::CCloud_rgb8(lCloud[i+1]));
            scene::PMatTransform lMatTransform=new scene::CMatTransform(lTrans);
            lSceneCloud->AttachTo(lMatTransform);
            scene::PCube lCube=new scene::CCube(0.1,0.1,0.1);
            lCube->AttachTo(lMatTransform);
            
            lScene->World()->AddChild(lMatTransform);
        }

        scene::PPerspectiveCamera lCamera=new scene::CPerspectiveCamera;
        lCamera->SetFOV(2*atan2(240.0,525.0));
        lCamera->SetClipping(0.1,50.0);
        lCamera->LookAt({0,0,1},{0,0,0},{0,-1,0});

        scene::PSceneView lSceneView=new scene::CSceneView;
        lSceneView->SetCamera(lCamera);
        lSceneView->AddScene(lScene);
        lSceneView->AddController(new scene::CCameraController(lCamera));
        lSceneView->Map(CSize_d(800,600));
        
        buola_main_loop();
    }
    catch(std::exception &pE)
    {
        msg_info() << "exception caught: " << pE.what() << "\n";
    }
}
